//
// Created by cyq on 2022/12/5.
//

#include "driver_chassis.h"
#include "math_basic.h"
#include "math.h"
#include "task_lostcounter.h"
#include "bsp_can.h"
#include "driver_superc.h"
#include "bsp_judge.h"
#include "ramp.h"
#define ChangePowerIncrease 0.0001f//(0.0002f)
#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x)))

#define Chassis_RAM5P_TIME 10

int flag;
float power_gap;
float ChangePowerK;
Limit_Power_Mode Now_Limit_Mode = Normal;
Limit_Power_Mode Last_Limit_Mode = Normal;
ChassisControlDataStruct chassisControlData;

ChassisControlDataStruct gimbalControlData;

ChassisRealDataStruct chassisRealData;
ChassisRealDataStruct gimbalRealData;

FollowControlDataStruct followControlData;

ChassisPowerControlDataStruct chassisPowerController;

ChassisControlDataStruct RawChassisSpeed;
uint8_t Uphill_flag;

ChassisSpeedDataStruct SpeedLimit;

float power_kd;

float testspeedd;

void ChassisMotorInit(MotorStruct* motor)
{
    motor->PIDLocation.calc = &PidCalc;
    motor->PIDLocation.clear = &PidClear;
    motor->PIDLocation.calc2 = &PidCalc2;
    motor->PIDLocation.Kp = CHASSISMOTOR_LOCTION_KP;
    motor->PIDLocation.Ki = CHASSISMOTOR_LOCTION_KI;
    motor->PIDLocation.Kd = CHASSISMOTOR_LOCTION_KD;
    motor->PIDLocation.OutMax = CHASSISMOTOR_LOCTION_OUTMAX;
    motor->PIDLocation.UiOutMax = CHASSISMOTOR_LOCTION_UIOUTMAX;
    motor->PIDLocation.clear(&motor->PIDLocation);

    motor->PIDSpeed.calc = &PidCalc;
    motor->PIDSpeed.clear = &PidClear;
    motor->PIDSpeed.calc2 = &PidCalc2;
    motor->PIDSpeed.Kp = CHASSISMOTOR_SPEED_KP;
    motor->PIDSpeed.Ki = CHASSISMOTOR_SPEED_KI;
    motor->PIDSpeed.Kd = CHASSISMOTOR_SPEED_KD;
    motor->PIDSpeed.OutMax = CHASSISMOTOR_SPEED_OUTMAX;
    motor->PIDSpeed.UiOutMax = CHASSISMOTOR_SPEED_UIOUTMAX;
    motor->PIDSpeed.clear(&motor->PIDSpeed);
    motor->Chassis_RAMP.count = 0;
    motor->Chassis_RAMP.scale = 0;
    motor->Chassis_RAMP.out = 0;
    motor->Chassis_RAMP.init = &ramp_init;
    motor->Chassis_RAMP.calc = &ramp_calc;
    motor->ax = 0;
    motor->bx = 0;
}
void PidInit(MotorStruct* Motor,float kp,float ki,float kd){
    Motor->PIDSpeed.Kp = kp;
    Motor->PIDSpeed.Ki = ki;
    Motor->PIDSpeed.Kd = kd;
}

void SpeedLimitInit(){
    SpeedLimit.RemoteK = 0.3f; //0.3f;
    SpeedLimit.RemoteIncreaseK = 0.005f; //0.000003f; 0.0001
    SpeedLimit.PowerLimitSpinDefault = 0.25; //0.35;
}
/**
 * 功率环这块的原理和老代码差不多，我把原来的积分项加上了kp和kd，效果更好点
 */
void PowerControlInit()
{
    uint8_t i;
    chassisPowerController.paraHinit = 6.7;//6.2f;     //功率参数初始化的起始值
    for (i = 1; i <= 4; i++)
    {
        chassisPowerController.paraH[i] = chassisPowerController.paraHinit;
        chassisPowerController.paraR[i] = 0.0005f;//0.00045f;
    }
    //todo:
    chassisPowerController.paraHinc = 0.0008f;
    chassisPowerController.paraHkp = 0.02f;
    chassisPowerController.paraHsumi = 0.0f;

    chassisPowerController.limitPower = 0.0f;

    chassisPowerController.nowPower = 0.0f;
}

void FollowControlInit(FollowControlDataStruct* control)
{
    control->PIDSpeed.calc = &PidCalc;
    control->PIDSpeed.calc2 = &PidCalc2;
    control->PIDSpeed.clear = &PidClear;
    control->PIDSpeed.Kp = CHASSISMOTOR_FOLLOW_ANGLE_SPEED_KP;
    control->PIDSpeed.Ki = CHASSISMOTOR_FOLLOW_ANGLE_SPEED_KI;
    control->PIDSpeed.Kd = CHASSISMOTOR_FOLLOW_ANGLE_SPEED_KD;
    control->PIDSpeed.clear(&control->PIDSpeed);
    control->PIDSpeed.UiOutMax = CHASSISMOTOR_FOLLOW_ANGLE_SPEED_UIOUTMAX;
    control->PIDSpeed.OutMax = CHASSISMOTOR_FOLLOW_ANGLE_SPEED_OUTMAX;

    control->PIDLocation.calc = &PidCalc;
    control->PIDLocation.calc2 = &PidCalc2;
    control->PIDLocation.Kp = CHASSISMOTOR_FOLLOW_LOCATION_KP;
    control->PIDLocation.clear = &PidClear;
    control->PIDLocation.Ki = CHASSISMOTOR_FOLLOW_LOCATION_KI;
    control->PIDLocation.Kd = CHASSISMOTOR_FOLLOW_LOCATION_KD;
    control->PIDLocation.OutMax = CHASSISMOTOR_FOLLOW_LOCATION_OUTMAX;
    control->PIDLocation.UiOutMax = CHASSISMOTOR_FOLLOW_LOCATION_UIOUTMAX;
    control->PIDLocation.clear(&control->PIDLocation);
    control->Location.SetLocation = GIMBAL_RESET_YAW_ENCODER;

}

int Error_Flag[5] = {0};
float JudgeChassisMotorError(){
    uint8_t Reach_Flag = 1;
    if(chassisControlData.chassisMode != CHASSIS_MODE_GYRO) {
        //这里是为了防止出现超级电容模式下也那么缓慢得启动的现象
        if(chassisControlData.disCharging == 1){
            for (int i = 1; i <= 4; i++) {
                if (fabsf(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) >= 0.3f)
                    return -1.0f;
            }
            for(int i = 3; i <= 6; i++) {
                if ((GetErrorState() >> i) & 0x0001) {
                    Error_Flag[i - 2] = 1;
                }
            }
            Reach_Flag = 1;
            for(int i = 1;i <= 4; i++){
                if((fabsf(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) > 0.1f) && (!Error_Flag[i]))
                    Reach_Flag = 0;
            }
            if(Reach_Flag)
                return 1.0f;
            return 0;
        }
        else
        {
            for (int i = 1; i <= 4; i++) {
                if (fabsf(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) >= 0.25f)
                    return -2.0f;
            }
            for(int i = 3; i <= 6; i++) {
                if ((GetErrorState() >> i) & 0x0001) {
                    Error_Flag[i - 2] = 1;
                }
            }
            Reach_Flag = 1;
            for(int i = 1;i <= 4; i++){
                if((fabsf(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) > 0.05f) && (!Error_Flag[i]))
                    Reach_Flag = 0;
            }
            if(Reach_Flag)
                return 1.0f;
            return 0;
        }
    }
    else{
        if(fabsf(chassisPowerController.limitPower - chassisPowerController.nowPower) >= GYRO_POWER_GAP){
            return -2.0f;
        }
        else{
            return 1.0f;
        }
    }
}

///这里只做到了在速度增加的时候，能够起到比较好的缓加速作用，但是假如在减速的过程中，就无法起到很好的作用，导致出现打滑的现象
void PowerControlSpeed(){
    chassisPowerController.Buffer = GetPowerBuffer();
    chassisPowerController.limitPower = GetPowerLimit();
    chassisPowerController.nowPower = GetPowerNow();

    if(chassisControlData.chassisMode != CHASSIS_MODE_GYRO){
        float speedsum = 1.2f * fabsf(chassisControlData.speedX) + 1.2f * fabsf(chassisControlData.speedY);

        if(speedsum > 0.01)
            SpeedLimit.RemoteK += JudgeChassisMotorError() * SpeedLimit.RemoteIncreaseK;
        else
            SpeedLimit.RemoteK = 0.40f;
        //TODO:考虑动态的去调整RemoteIncreaseK,以及这个PowerControlSpeed（）里面的很多参数都是很可能会随着车子的改变而变化的，所以需要多进行测试
        float RemoteK_temp1 = chassisPowerController.limitPower / 100.0f + 0.2;
        if(SpeedLimit.RemoteK > RemoteK_temp1)
            SpeedLimit.RemoteK = RemoteK_temp1;
        if(SpeedLimit.RemoteK > 1)
            SpeedLimit.RemoteK = 1;
        if(chassisControlData.disCharging == 1) {
            if (SpeedLimit.RemoteK < 0.3f)
                SpeedLimit.RemoteK = 0.3f;
        }
        else if(chassisControlData.disCharging != 1){
            if (SpeedLimit.RemoteK < 0.1f)
                SpeedLimit.RemoteK = 0.1f;
        }
        if(chassisControlData.UphillFlag)
            SpeedLimit.RemoteK = 1;
        chassisControlData.speedX = chassisControlData.speedX * SpeedLimit.RemoteK;
        chassisControlData.speedY = chassisControlData.speedY * SpeedLimit.RemoteK;
    }
    //由于小陀螺模式下，速度的偏差一直会非常得大，所以我们不能再继续沿用前面其他模式下对于速度模式的限幅
    else if(chassisControlData.chassisMode == CHASSIS_MODE_GYRO){
        SpeedLimit.RemoteK += JudgeChassisMotorError() * SpeedLimit.RemoteIncreaseK * (0.05f);
        if(SpeedLimit.RemoteK < 0.3f)
            SpeedLimit.RemoteK = 0.3f;
        else if(SpeedLimit.RemoteK > 1)
            SpeedLimit.RemoteK = 1;
        chassisControlData.speedX = chassisControlData.speedX * SpeedLimit.RemoteK;
        chassisControlData.speedY = chassisControlData.speedY * SpeedLimit.RemoteK;
    }
}

float GetPowerNow()
{
    //假如电容板寄了，就直接使用裁判系统返回的数据
    if ((GetErrorState() >> SUPERC_LOST_COUNT) & 0x0001) {
        return judge_rece_mesg.power_heat_data.chassis_power;
    }
        //得到四个轮上电机的功率
    else
        return SC_get_power_wheel();
}

float GetPowerBuffer()
{
    if((GetErrorState() >> JUDGEMENT_LOST_COUNT) & 0x0001)
        return 25;
//  如果电容在放电
    return judge_rece_mesg.power_heat_data.chassis_power_buffer;
}

float last_limit_power = 0;

float last_limit_power_discharging = 0;

//根据模式来限制功率
float _limit_power;
float Power_Increase = 0.f;
float GetPowerLimit()
{
    Last_Limit_Mode = Now_Limit_Mode;

    //收不到裁判系统
    //TODO:不应该在收不到裁判系统数据的时候只给到50w，考虑这部分应该怎么写才能在不减鲁棒性的同时最大利用功率
    if ((GetErrorState() >> JUDGEMENT_LOST_COUNT) & 0x0001) {
        if((last_limit_power != 0) && (Last_Limit_Mode != Discharging))
            _limit_power = last_limit_power;
        else if((last_limit_power != 0) && (Last_Limit_Mode == Discharging) )
            if(chassisControlData.disCharging)
                _limit_power = last_limit_power_discharging;
            else
                _limit_power = last_limit_power;
        else
            _limit_power = 50;

        Now_Limit_Mode = Lost;
    }

    //电容放电
    //explanation：在很多情况下这样的放电功率是冗余的，我们没有必要给到这么高的限制功率，因为等级低的时候，功率是很低的，
    //大部分功率就会需要由裁判系统来提供，这种情况下，电容的续航很差，无法支持我们跑很久
    //todo：联盟赛修改了一下大小，到时候再修改回来
    else if(chassisControlData.disCharging) {
//        LIMIT(ChangePowerK,0,(logf(3.f) * 0.5f));
//        _limit_power = (float)(judge_rece_mesg.game_robot_state_data.chassis_power_limit * (exp(2 * ChangePowerK)));
//        LIMIT(ChangePowerK,0,(logf(2.f)));
        _limit_power = (float)(judge_rece_mesg.game_robot_state_data.chassis_power_limit + 110.0f);
        //test
        Now_Limit_Mode = Discharging;
        last_limit_power_discharging = _limit_power;
        ChangePowerK += ChangePowerIncrease;
    }

    //AUTO_SuperC_Mode
    //先前的120w有点太高了，降低一点延长使用时间
    ///将这部分代码改成跟缓冲能量有关系的一个值
    else if(chassisControlData.disCharging_Auto){
        Power_Increase = 30.0f + (float)(judge_rece_mesg.power_heat_data.chassis_power_buffer - 60);
        if(Power_Increase < 0.f)
            Power_Increase = 0;
        _limit_power = (float)judge_rece_mesg.game_robot_state_data.chassis_power_limit + Power_Increase;
    }
    else {
        //正常情况
        _limit_power = judge_rece_mesg.game_robot_state_data.chassis_power_limit;
        last_limit_power = _limit_power;
        Now_Limit_Mode = Normal;
    }
    return _limit_power;
}


float test_set_speedX = 0.0f;
float test_set_speedY = 0.0f;
uint8_t test_set_superFlag = 0;
void ChassisMotorSetSpeed(void)
{
//    ChassisMotor[1].Speed.SetSpeed =
//            (0.0f - chassisControlData.speedY + chassisControlData.speedX + chassisControlData.speedSpin);
//
//    ChassisMotor[2].Speed.SetSpeed =
//            (0.0f + chassisControlData.speedY + chassisControlData.speedX + chassisControlData.speedSpin);
//
//    ChassisMotor[3].Speed.SetSpeed =
//            (0.0f + chassisControlData.speedY - chassisControlData.speedX + chassisControlData.speedSpin);
//
//    ChassisMotor[4].Speed.SetSpeed =
//            (0.0f - chassisControlData.speedY - chassisControlData.speedX + chassisControlData.speedSpin);
    ChassisMotor[1].Speed.SetSpeed =
            (0.0f + chassisControlData.speedY - chassisControlData.speedX - chassisControlData.speedSpin);

    ChassisMotor[2].Speed.SetSpeed =
            (0.0f - chassisControlData.speedY - chassisControlData.speedX - chassisControlData.speedSpin);

    ChassisMotor[3].Speed.SetSpeed =
            (0.0f - chassisControlData.speedY + chassisControlData.speedX - chassisControlData.speedSpin);

    ChassisMotor[4].Speed.SetSpeed =
            (0.0f + chassisControlData.speedY + chassisControlData.speedX - chassisControlData.speedSpin);
    uint8_t i;
    //缩放（把四个轮子的速度都归到0——1的范围之内）
    float suofangK = 1.0f;
    for (i = 1; i <= 4; i++)
    {
        if (fabsf(ChassisMotor[i].Speed.SetSpeed) > suofangK)
            suofangK = fabsf(ChassisMotor[i].Speed.SetSpeed);
    }

    for (i = 1; i <= 4; i++)
    {
        ChassisMotor[i].Speed.SetSpeed /= suofangK;
    }
    for (i = 1; i <= 4; i++)
    {
        ChassisMotor[i].PIDMode = MOTOR_PID_MODE_SPEED;
        //限幅
        ChassisMotor[i].Speed.SetSpeed = AmplitudeLimit(ChassisMotor[i].Speed.SetSpeed, 1.0f);
    }
}

float lastChassisPower;
float PowerBuffer;
float H_kd = 0;
float PowerControlCalculate()
{
    //在限制之上稍微加上一点
    chassisPowerController.limitPower = GetPowerLimit();
    //在没有电容板返回值的情况下，读取裁判系统返回的功率值
    chassisPowerController.nowPower = GetPowerNow();
    //根据缓冲能量稍微改变它的限制功率
    PowerBuffer = GetPowerBuffer();
    //在我的理解中这个函数主要还是用来保护的，60j的缓冲能量其实起不到太大的加速作用
    //虽然之前的线性曲线可以让功率在一瞬间达到一个比较大的值，但事实上是起不到太大的加速作用的（作用时间过短,而且从能量守恒的角度也很容易知道这点）
    //同时，由于曲线是线性的，所以缓冲能量的消耗对于功率造成的效果始终相似，而新的函数能够让曲线在值比较小的时候，降低的更快，高的时候高消耗更持久一些
    //并使得更贴合目标曲线
    if(chassisControlData.chassisMode != CHASSIS_MODE_GYRO)
        chassisPowerController.limitPower = chassisPowerController.limitPower * (0.82f * logf(PowerBuffer) - 2.0f);
    ///小陀螺模式下用了另一个功率曲线（因为 小陀螺时，其速度增加、减少的较慢，如果是加速到比较大的一个功率又在降下来的话，其实没太大意义，同时还会影响运动）
    else
        chassisPowerController.limitPower = chassisPowerController.limitPower * (0.6f * logf(PowerBuffer) - 1.4f);
    //功率限制太小，就干脆不给功率了，求稳
    if (chassisPowerController.limitPower < 0)
        chassisPowerController.limitPower = 0;
    //然后开始算出K
    float resK = 0;
    float qA = 0, qB = 0, qC = chassisPowerController.limitPower;
    float tempB,tempA;
    uint8_t i;
    for (i = 1; i <= 4; i++)
    {
        //RI^2
        tempA = chassisPowerController.paraR[i] * ChassisMotor[i].PIDSpeed.Out * ChassisMotor[i].PIDSpeed.Out;
        qA += tempA;
        //HIV
        tempB = chassisPowerController.paraH[i] * ChassisMotor[i].PIDSpeed.Out * ChassisMotor[i].Speed.Speed;
        qB += tempB;
    }
    chassisPowerController.qA = qA;
    chassisPowerController.qB = qB;
    chassisPowerController.qC = qC;

    testspeedd = ChassisMotor[3].Speed.Speed;
    if (qA == 0.0f) {
        resK = 1.0f;
    }
    //通过限幅的功率算出能最大限度有效使用到功率的k值
    else {
        resK = (float )(-qB + sqrtf(qB * qB + 4.0f * qA * qC / 80.0f)) / (2.0f * qA);
    }
    if (resK > 1) {
        resK = 1;
    }
    else
    {
        if (qA != 0 && qB != 0)
        {
            float powerErr = chassisPowerController.nowPower - chassisPowerController.limitPower;
            chassisPowerController.paraHsumi += chassisPowerController.paraHinc * powerErr;
            //限幅chassisPowerController.paraHsumi
            float _paraHsumi_min = -0.5f;//-0.3f;
            float _paraHsumi_max = 7.0f;//10.0f;
            if (chassisPowerController.paraHsumi < _paraHsumi_min)
                chassisPowerController.paraHsumi = _paraHsumi_min;
            else if (chassisPowerController.paraHsumi > _paraHsumi_max)
                chassisPowerController.paraHsumi = _paraHsumi_max;

            for (i = 1; i <= 4; i++)
            {
                chassisPowerController.paraH[i] =
                        chassisPowerController.paraHinit + chassisPowerController.paraHkp * powerErr +
                        chassisPowerController.paraHsumi + H_kd * (chassisPowerController.nowPower - lastChassisPower);
                lastChassisPower = chassisPowerController.nowPower;

                float _paraH_min = 5.0f;//6.0f;
                float _paraH_max = 30.0f;//22.0f;
                if (chassisPowerController.paraH[i] < _paraH_min)
                    chassisPowerController.paraH[i] = _paraH_min;
                else if (chassisPowerController.paraH[i] > _paraH_max)
                    chassisPowerController.paraH[i] =_paraH_max;
                if((GetErrorState() >> JUDGEMENT_LOST_COUNT) & 0x0001)
                {
                    chassisPowerController.paraH[i] = 6.5f;
                    chassisPowerController.paraR[i] = 0.0005f;
                }
            }
        }
    }

    if (resK < 0)
        resK = 0;

    return resK;
}
int tempjudge = 0;
float tempf = 0;
float tempi = 0;
//是否在上坡的一个判断，如果返回值为1，说明头朝弹仓，返回值为2说明头朝另一边，返回值为0说明没有在上坡
int UpHillJudge(){
    tempf = followControlData.Location.SetLocation - GIMBAL_RESET_YAW_ENCODER;
    tempi = (int)tempf;
    //tempjudge这个值为 1说明是朝着弹仓方向前进，不然就是在朝着另一边前进
    if(fabsf(tempf - tempi) < 0.001f)
        tempjudge = 1;
    else
        tempjudge = 0;
    if(tempjudge == 1)
    {
        if(INS.Pitch < -UPHILL){
            chassisControlData.UphillFlag = 1;
            return 1;
        }

        else{
            chassisControlData.UphillFlag = 0;
            return 0;
        }
    }

    if(tempjudge == 0){
        if(INS.Pitch > UPHILL){
            chassisControlData.UphillFlag = 1;
            return 2;
        }

        else{
            chassisControlData.UphillFlag = 0;
            return 0;
        }
    }
    return 0;
}
int tempjudge_uphill = 0;
void ChassisMotorCalculate(void)
{
    uint8_t i;
    for (i = 1; i <= 4; i++) {
        MotorPIDCalculate(&ChassisMotor[i]);
    }
    chassisPowerController.k = PowerControlCalculate();
    for (i = 1; i <= 4; i++) {
        ChassisMotor[i].PIDSpeed.Out *= chassisPowerController.k;
    }
    tempjudge_uphill = UpHillJudge();
    if((chassisControlData.chassisMode == CHASSIS_MODE_FOLLOW) && chassisControlData.UphillFlag) {
        if (tempjudge_uphill == 1){
            ChassisMotor[1].PIDSpeed.Out *= FRONT;
            ChassisMotor[2].PIDSpeed.Out *= FRONT;
            ChassisMotor[3].PIDSpeed.Out *= REAR;
            ChassisMotor[4].PIDSpeed.Out *= REAR;
        }
        else if(tempjudge_uphill == 2){
            ChassisMotor[1].PIDSpeed.Out *= REAR;
            ChassisMotor[2].PIDSpeed.Out *= REAR;
            ChassisMotor[3].PIDSpeed.Out *= FRONT;
            ChassisMotor[4].PIDSpeed.Out *= FRONT;
        }
    }
}
uint8_t CAN1SendMessage[8] = { 0 };
void ChassisMotorDataSend(void)
{
    uint8_t i;
    if (chassisControlData.chassisMode == CHASSIS_MODE_FREE) {
        for (i = 0; i < 8; i++) {
            CAN1SendMessage[i] = 0;
        }
    }
    else {
        CAN1SendMessage[0] = ((int16_t)(ChassisMotor[1].PIDSpeed.Out * 16384)) >> 8 & 0x00ff;
        CAN1SendMessage[1] = ((int16_t)(ChassisMotor[1].PIDSpeed.Out * 16384)) & 0x00ff;
        CAN1SendMessage[2] = ((int16_t)(ChassisMotor[2].PIDSpeed.Out * 16384)) >> 8 & 0x00ff;
        CAN1SendMessage[3] = ((int16_t)(ChassisMotor[2].PIDSpeed.Out * 16384)) & 0x00ff;
        CAN1SendMessage[4] = ((int16_t)(ChassisMotor[3].PIDSpeed.Out * 16384)) >> 8 & 0x00ff;
        CAN1SendMessage[5] = ((int16_t)(ChassisMotor[3].PIDSpeed.Out * 16384)) & 0x00ff;
        CAN1SendMessage[6] = ((int16_t)(ChassisMotor[4].PIDSpeed.Out * 16384)) >> 8 & 0x00ff;
        CAN1SendMessage[7] = ((int16_t)(ChassisMotor[4].PIDSpeed.Out * 16384)) & 0x00ff;
    }
    CAN1_Send_Msg(CAN1SendMessage, 8, 0x200);
}

/**
 * @brief referenced in CAN callback function, in bsp_can.c，这个函数把gimbalControlData的数据转移到chassisControlData，实际上只转移了mode和superflag，运动的计算在对应的模式里
 * gimbalcontrolData的速度是相对于云台的，chassiscontroldata的速度是在底盘坐标系的
 * @param controlData
 */
void ChassisControlDataUpdate(ChassisControlDataStruct* controlData)
{
    chassisControlData.chassisMode = controlData->chassisMode;

    if ((GetErrorState() >> JUDGEMENT_LOST_COUNT) & 0x0001) {
        chassisControlData.superFlag = 0;
    }
    else {
        chassisControlData.superFlag = controlData->superFlag;
    }
}

/**
 * @brief Use yaw encoder data to update FollowControlData
 * @param motor
 */
void FollowControlDataUpdate(DM_MotorStruct * motor)
{
    followControlData.Location.Location = motor->Location.Location;
    followControlData.Speed.Speed = INS.Gyro[2];
}

void ChassisFollowSetSpeed()
{
    ChassisGyroSetSpeed();
    followControlData.PIDLocation.Ref = GIMBAL_RESET_YAW_ENCODER;
    followControlData.PIDLocation.Fdb = (float)(yawMotor.Location.Location - (int)yawMotor.Location.Location);
    //处理过零点问题
    if (followControlData.PIDLocation.Fdb < followControlData.PIDLocation.Ref - 0.5f)
        followControlData.PIDLocation.Fdb += 1.0f;

    if (followControlData.PIDLocation.Fdb > followControlData.PIDLocation.Ref + 0.5f)
        followControlData.PIDLocation.Fdb -= 1.0f;

    followControlData.PIDLocation.calc(&followControlData.PIDLocation);
    chassisControlData.speedSpin = -followControlData.PIDLocation.Out;
}
float rotation_angle = 0.f;
void ChassisGyroSetSpeed()
{
    float limit = (GetPowerLimit()) * (0.82f * logf(PowerBuffer) - 2.0f);

    float TempSpin = 0.4f;
    if(chassisControlData.disCharging_Auto)
        TempSpin = 0.4f;
    else
        TempSpin = TempSpin + (GetPowerLimit() / judge_rece_mesg.game_robot_state_data.chassis_power_limit) / 10.f;
    if(TempSpin > 0.6f)
        TempSpin = 0.6f;
    if(TempSpin < 0.35f)
        TempSpin = 0.35f;
    chassisControlData.speedSpin = TempSpin;
    rotation_angle = followControlData.Location.Location - GIMBAL_RESET_YAW_ENCODER;
    rotation_angle = (float)(rotation_angle - (int)rotation_angle);

    float tempSpeedX = 0.f, tempSpeedY = 0.f;
    tempSpeedX = gimbalControlData.speedX;
    tempSpeedY = gimbalControlData.speedY;

    chassisControlData.speedX =
            RotationSoluteX(rotation_angle, tempSpeedX, tempSpeedY);

    chassisControlData.speedY =
            RotationSoluteY(rotation_angle, tempSpeedX, tempSpeedY);

    PowerControlSpeed();
}


/**
 * @brief 这个函数根据电机速度数据反馈真实的speedx、speedy和speedspin，在键鼠的斜坡中有应用，调试的时候也用得到
 * @brief chassisrealdata是底盘坐标系下的速度，gimbalrealdata是云台坐标系下的速度
 */
void ChassisSpeedFeedback()
{
    chassisRealData.speedX = (-ChassisMotor[1].Speed.Speed - ChassisMotor[2].Speed.Speed + ChassisMotor[3].Speed.Speed +
                              ChassisMotor[4].Speed.Speed) / 4.0f ;

    chassisRealData.speedY = (ChassisMotor[1].Speed.Speed - ChassisMotor[2].Speed.Speed - ChassisMotor[3].Speed.Speed +
                              ChassisMotor[4].Speed.Speed) / 4.0f ;
    chassisRealData.speedSpin =
            (ChassisMotor[1].Speed.Speed/ 4.0f + ChassisMotor[2].Speed.Speed / 4.0f+ ChassisMotor[3].Speed.Speed/ 4.0f +
             ChassisMotor[4].Speed.Speed/ 4.0f);

    float RotationAngle = GIMBAL_RESET_YAW_ENCODER - yawMotor.Location.Location;
    RotationAngle = (float)(RotationAngle - (int)(RotationAngle));

    gimbalRealData.speedX =
            RotationSoluteY(RotationAngle, chassisRealData.speedY, chassisRealData.speedX);
    gimbalRealData.speedY =
            RotationSoluteX(RotationAngle, chassisRealData.speedY, chassisRealData.speedX);
}